'**********************************  EtherCAT Setup Program  **********************************
' File:        EC_SETUP.PRG
' Purpose:     EtherCAT setup example
' Version:     V1.0.0.8
' Author:      Nir Geller
' Description: This script demonstrates how to setup the EtherCAT module, which function/subroutines to invoke,
'              and in what order. 
'
' History:  20.10.2013  V1.0.0.0 - Created
'
'      15.12.2014  V1.0.0.1 - Adapted to comply with new IOs handling
'      in Ethercat.LIB version 1.0.0.2   ,   MC version 0.4.13.2rc1-C2
'
'      28.12.2014  V1.0.0.2 - EC_STARTMASTER waits for hpet calibration. EC_SETUP.PRG no longer does that.
'      Added user functions for PEROP, SAFEOF and OP configuration.
'
'      5.1.2015  V1.0.0.4 - Creation of PDO mapping and bus cycle time setting is moved from EC_SETUP.PRG into the context of EC_STARTMASTER
'
'      26.4.2015  V1.0.0.5 - If EC_STARTMASTER throws a PLL error, EC_SETUP.PRG is restarted up to 5 times retrying to finish successfully
'                   CHECK_TOPOLOGY added to ecat setup sequence
'
'      28.1.2016  V1.0.0.6 - Use constant for BUS identification: CAN and EtherCAT
'
'      12.4.2016  V1.0.0.7 - Adapted to work with AXLFBKEC.LIB instead of the deprecated EC_AI8ME.LIB
'
'      28.12.2016  V1.0.0.8 - Added cases for CDHD product code STX_CDHD_PRODUCT_CODE_0X1
'      06.05.2021 v1.0.0.1 - Added cases for QNDC_CDRD_VENDOR_ID_0X748 -- Rocky Qiu
'
'**********************************************************************************************


'module global "constants"

'module global variables
common shared globalCycleTime as long = 4000 '2000 '1000

common shared numOfSlaves as long = 0
common shared num_Of_Motion_Drives as long = 0

dim shared num_Of_Digital_Inputs as long = 4
dim shared num_Of_Digital_Outputs as long = 4

'dim shared Note_PLL_Error_Retry as note "PLL Error in the drives. Restarting EC_SETUP.PRG" 20300
'common shared ERR_EC_SETUP as error "ERROR SETTING UP ETHERCAT. CHECK EC_SETUP_PRG_error_code/EC_SETUP_PRG_error_STR" 20764

Program 

  dim Axis_Attached_To_Drive as long = 1
  dim Counts_Per_Revolution as long = 10000  '  Set to 10000 by default for a generic drive
  dim ec_setup_iterations as long = 1
  dim retVal as long = 0
  dim drive_addr as long = 0
  dim PLL_Error as long = 0
  dim i as long = 0
  dim defaultMotionSlaveNumber as long = 2


  ' set QNDC PDO mapping
  for drive_addr = 1 to defaultMotionSlaveNumber
      call EC_REMAP_QNDC_CDRD(drive_addr)
  next   

  START_EC_SETUP_PRG:

  numOfSlaves = 0
  num_Of_Motion_Drives = 0
  Axis_Attached_To_Drive = 1
  Counts_Per_Revolution = 10000  '  Set to 10000 by default for a generic drive
  retVal = 0
  drive_addr = 0
  PLL_Error = 0

  numOfSlaves = EC_ETHERCAT_INIT ' Initialize EtherCAT according to a certain field bus, and type of slave addressing

  ' In CDHD motion drives, remap PDOS according to softMC requirements
  ' for drive_addr = 1 to numOfSlaves

  '   if ETSLAVES[drive_addr]->et_slavetype = ECAT_MOTIONSLAVE then

  '     if EC_SLAVES_INFO[drive_addr]->vendor_id = SERVOTRONIX_VENDOR_ID_0X2E1 then

  '       select case EC_SLAVES_INFO[drive_addr]->product_code

  '         case STX_CDHD_PRODUCT_CODE_0X0, STX_CDHD_PRODUCT_CODE_0X1
  '           if EC_SDO_READ(1,0x20C3,0) <> 1 then
  '               call EC_REMAP_CDHD_DEFAULT_PDOS(drive_addr)
  '           end if

  '       end select  'EC_SLAVES_INFO[drive_addr]->product_code

  '     end if

  '     if EC_SLAVES_INFO[drive_addr]->vendor_id = QNDC_CDRD_VENDOR_ID_0X748 then

  '       select case EC_SLAVES_INFO[drive_addr]->product_code

  '           case QNDC_CDRD_PRODUCT_CODE_0X0A

  '               call EC_REMAP_QNDC_CDRD(drive_addr)
                
  '       end select

  '     end if

  '   end if

  ' next

  retVal = EC_RESCAN_SLAVES
  sleep 1000

  ' Check the EtherCAT topology for miswiring
'   retVal = CHECK_TOPOLOGY  '  throws error
  call EC_CREATE_MASTER
  print "Found ";numOfSlaves;" slaves"

  call EC_SET_BUS_CYCLETIME(globalCycleTime)  '  For a cycle time of 500 us, add to FWCONFIG: hpetfreq = 2000

  EC_ADDRESS_BY_ALIAS = 0 ' Set to zero -> alias addressing is position based


  '****************************************************************
  '                                *
  '        PRE - OPERATIONAL CONFIGURATION          *
  ' Configure  each slave according to Vendor ID and Product Code  *
  '                                *
  '****************************************************************

	for drive_addr = 1 to numOfSlaves
		Select Case EC_SLAVES_INFO[drive_addr]->vendor_id

		 Case SERVOTRONIX_VENDOR_ID_0X2E1, PBA_SYS_MAXTUNE_VENDOR_ID, GOOGOLTECH_GTHD_VENDOR_ID, MPC_FPRO_VENDOR_ID, AKRIBIS_ASD_VENDOR_ID, TECO_JSDZ_VENDOR_ID, HIGEN_SDA7_VENDOR_ID, SCHNEIDER_VENDOR_ID, QNDC_CDRD_VENDOR_ID_0X748

			select case EC_SLAVES_INFO[drive_addr]->product_code

			  case STX_CDHD_PRODUCT_CODE_0X0, STX_CDHD_PRODUCT_CODE_0X1

          call EC_INSTALL_STX_CDHD(drive_addr, EC_SLAVES_INFO[drive_addr]->vendor_id, EC_SLAVES_INFO[drive_addr]->product_code, CDHD_OPMODE_SYNC_POSITION, 2)  '  mic_int = 2
          num_Of_Motion_Drives = num_Of_Motion_Drives + 1

			  ' case MAXX_BOARD_PRODUCT_CODE

				'   call EC_INSTALL_MAXX(drive_addr)
				
				case QNDC_CDRD_PRODUCT_CODE_0X0A
			  
					call EC_INSTALL_QNDC_CDRD(drive_addr)
          num_Of_Motion_Drives = num_Of_Motion_Drives + 1
					  
			end select
		end select  '  SEC_SLAVES_INFO[drive_addr]->product_code

	next

  ' Run IO_START initalizing addional IO's
  try 
    ktas io_start.prg
    unload io_start.prg
    catch ELse
  end try
  load IO_START.PRG
  stas IO_START.PRG
  while IO_Start.prg.state <> 10
  sleep 10
  end while

  ' Create PDOs mapping, set bus cycle time, sync clocks between the master and the slaves,
  ' raise slaves to OP-Mode and sets master's opmode to operational

  call EC_ASSIGN_PDOS_AUTO
  if (EC_PDOEntryNumber[1] < EC_PDOAssignTotalNumber) OR (EC_PDOEntryNumber[2] < EC_PDOAssignTotalNumber) then 
    goto START_EC_SETUP_PRG
  end if 

  sleep 3000  ' Sleep to avoid EC_STARTMASTER failed
  call EC_STARTMASTER


  '****************************************************************
  '                                *
  '           OPERATIONAL CONFIGURATION          *
  ' Configure each slave according to Vendor ID and Product Code  *
  '                                *
  '****************************************************************

  for drive_addr = 1 to numOfSlaves

    ' SAFEOP state configuration - Add code to EC_USER_SAFEOP_CONFIG(byval drive_addr as long)
    retVal = EC_USER_SAFEOP_CONFIG(drive_addr)

    ' OP state configuration - Add code to EC_USER_OP_CONFIG(byval drive_addr as long)
    retVal = EC_USER_OP_CONFIG(drive_addr)

    if TRUE = ETSLAVES[drive_addr]->et_in_use and ECAT_MOTIONSLAVE = ETSLAVES[drive_addr]->et_slavetype then

      ' Assign slave address to each axis
      systemAxis(Axis_Attached_To_Drive).busnumber = ECAT_BUSID
      retVal = EC_SLAVE_GET_AXES_NUMBER(drive_addr)
      for i = 1 to retVal
        systemAxis(Axis_Attached_To_Drive).dadd = drive_addr
        systemAxis(Axis_Attached_To_Drive).NodeId = i
        Axis_Attached_To_Drive = Axis_Attached_To_Drive + 1
      next

      ' Attempt to clear faults in the drives
      Try

        call EC_CLEAR_FAULTS(drive_addr)

      Catch Else

        ' if clear faults failed due to PLL Sync Error, wait a little longer and retry
        if EC_SDO_READ(drive_addr, 0x603f, 0) = 0x7386 then
          printu "PLL Error in drive #. Sleeping 5 seconds and retrying to clear fault";drive_addr
          sleep 5000
          call EC_CLEAR_FAULTS(drive_addr)
        end if

      End Try

      ' Select Case EC_SLAVES_INFO[drive_addr]->vendor_id

      '   Case SERVOTRONIX_VENDOR_ID_0X2E1

      '     select case EC_SLAVES_INFO[drive_addr]->product_code

      '       case STX_CDHD_PRODUCT_CODE_0X0, STX_CDHD_PRODUCT_CODE_0X1

      '       ' Add additional code if required

      '     end select   ' EC_SLAVES_INFO[drive_addr]->product_code


      '   Case GENERIC_DS402_DRIVE_VENDOR

      '     if EC_SLAVES_INFO[drive_addr]->product_code = GENERIC_DS402_DRIVE_PRODUCT_ID then

      '     ' Add additional code if required

      '     end if
		
      '   Case QNDC_CDRD_VENDOR_ID_0X748
          
      '     if EC_SLAVES_INFO[drive_addr]->product_code = QNDC_CDRD_PRODUCT_CODE_0X0A then 
          
      '       '-------------- Set drives to sync position mode for CDRD drive ----------------------
            
      '       if EC_IS_PDO(drive_addr, Modes_Of_Operation_6060h, 0) then
      '       call EC_PDO_WRITE(drive_addr, Modes_Of_Operation_6060h, 0, 8)
      '       else
      '       call EC_SDO_WRITE(drive_addr, Modes_Of_Operation_6060h, 0, 8, 8)
      '       end if

      '       if EC_IS_PDO(drive_addr, Modes_Of_Operation_6060h + 0x800, 0) then
      '       call EC_PDO_WRITE(drive_addr, Modes_Of_Operation_6060h + 0x800, 0, 8)
      '       else
      '       call EC_SDO_WRITE(drive_addr, Modes_Of_Operation_6060h + 0x800, 0, 8, 8)
      '       end if
            
      '     end if
			
      ' End Select

    end if  '  ECAT_MOTIONSLAVE


  next  '  for drive_addr = 1 to numOfSlaves

  print "EtherCAT Setup Finished Successfully !"

End Program




